#include <math.h>
#include "mathutils.h"
#include "syncommon.h"
#include "synfx.h"


namespace Synth {

void Delay::setSize(int lengthInSamples) {
	delaySize = lengthInSamples & ~(BLOCKSIZE-1); //make sure it's divisible by BLOCKSIZE
	memset(delayBuffer, 0, delaySize*sizeof(float));
}

// a small, nice, efficient delay with feedback
void Delay::Render(Block buffer, float amount, float feedback) {
	//let's do 32 samples, as usual...
	FORBLOCK(i) {
		//read from the delay buffer
		float val = delayBuffer[delayPos+i];
		//send our input into the delay, along with some feedback
		delayBuffer[delayPos+i] = buffer[i] + val * feedback;
		//write the delayed sound to the input
		buffer[i] = amount*val;
	}

	delayPos += BLOCKSIZE;
	if (delayPos >= delaySize)
		delayPos = 0;
}

void SVFFilter::Render(Block buffer, float cutoff, float resonance, int type) {
	float cf = cutoff*cutoff;
	if (cf > 1.0f)  cf = 1.0f;
	if (cf < 0.04f) cf = 0.04f;
	if (resonance > .99f) resonance = .99f;
	float rez = 1.0f - resonance;
	float in;
	float *o[5];
	o[0] = &in;
	o[1] = &lo;
	o[2] = &hi;
	o[3] = &bp; 
	o[4] = &bs;
	o[0] = o[type];
#ifndef STREAMLINE
	if (type >= 5)
		DebugBreak();
#endif
	//cf=sCutoff;
	FORBLOCK(i) {
		lo = lo + cf * bp;
		in = buffer[i];
		hi = buffer[i] - lo - rez*bp;
		bp = cf * hi + bp; 
		bs = hi + lo;
		buffer[i] = o[0][0];
		//cf+=deltaCutoff;
	}
}

void SVFFilter::Reset() {
	lo = hi = bp = bs = 0;
}

const double _kd = 0.00000000000000001f;

void MoogFilter::Render(Block buf, float cutoff, float res) {
	cutoff *= 44100;
	float f = (cutoff + cutoff) / INTERNAL_SAMPLE_RATE;
	float p = f * (1.8 - 0.8*f);
	float k = 2 * p - 1.0;
	float t = (1.0 - p) * 1.386249;
	float t2 = 12.0 + t*t;
	float r = res * (t2 + 6.0*t) / (t2 - 6.0*t);
	FORBLOCK(i) {
		float x = buf[i] - r*y4;
		y1 = x*p  +  oldx*p - k*y1;
		y2 = y1*p + oldy1*p - k*y2;
		y3 = y2*p + oldy2*p - k*y3;
		y4 = y3*p + oldy3*p - k*y4;
		y4 = y4 - ((y4*y4*y4) / 6.0);
		oldx = x;
		oldy1 = y1 + _kd;
		oldy2 = y2 + _kd;
		oldy3 = y3 + _kd;
		buf[i] = y4;
	}
}

// the result is in outlp
// 1/ call MyTLP24DB.Process
// 2/then get the result from outlp.
// this filter have a fantastic sound w/a very special res
// _kd is the denormal killer value.


inline float pow4(float x) {
	return (x*x)*(x*x);
}

void Envelope::Render(Block buf) {
	if (phase == EP_DEAD) {
		memset(buf, 0, BLOCKSIZE * sizeof(float));
		return;
	}
	if (params->attack < 0.001)
		params->attack = 0.001;
	if (params->decay < 0)
		params->decay = 0;
	if (params->release < 0)
		params->release = 0;
	float a_coeff = 1 - exp(-1 / (10 * pow4(params->attack) * INTERNAL_SAMPLE_RATE));
	float d_coeff = 1 - exp(-1 / (10 * pow4(params->decay) * INTERNAL_SAMPLE_RATE));
	float r_coeff = 1 - exp(-1 / (10 * pow4(params->release) * INTERNAL_SAMPLE_RATE));
	FORBLOCK(i) {
		if (gate) {
			if (rising) {
				value += a_coeff * (1 + (1.0f / 0.63f) - value);
				if (value > 2) {
					value = 2; // clips really snap attacks
					rising = false;
					phase = EP_DECAY;
				}
			} else {
				value += d_coeff * (params->sustain + 1 - value);
			}
		} else {
			value += r_coeff * (1.0f - (1.0f / 0.63f) - value);
			if (value < 1) {
				value = 1;
				phase = EP_DEAD;
			}
		}
		buf[i] = (value - 1) * scale;
	}
}

void Envelope::KeyDown() {
	value = 1;
	rising = true;
	gate = true;
	phase = EP_ATTACK;
}

void Envelope::KeyUp() {
	phase = EP_RELEASE;
	gate = false;
}

void Distorter::Distort(float *buf, float amount, float foldback, int type) {
// X' = (3/2)X - (1/2)X^3
	// TODO - SSE
	float k = 2*amount/(1-amount);

	FORBLOCK(i) {
		/*
		if (buf[i] > 1.0f)
			buf[i] = 1.0f-(1.0f-buf[i])*foldback;
		else if (buf[i] < -1.0f)
			buf[i] = -1.0f+(-1.0f-buf[i])*foldback;
		buf[i] = sinf(buf[i]);
		*/
		float x = buf[i];
		x = (1+k)*x/(1+k*abs(x));
		switch (type) {
		case 1:
			{
				x -= x * x * x * foldback;
				/*
				float x = buf[i] / (1 - amount * 0.98);
				float fb = 0;
				if (x > 1) {fb = -1+x; x = 1 - fb * foldback;}
				if (x < -1) {fb = 1-x; x = -1 + fb * foldback;}
				buf[i] = x;
				*/
				//float x = buf[i] * (1 + amount);
				//x -= x * x * x * foldback;
				//buf[i] = x;
			}
			break;
		case 2:
			{
				float multiplier = (foldback * foldback) * 128 + 2;
				float ix = x * multiplier;
				int intagah = ix;
				x = (float)intagah / multiplier;
			//float x = buf[i]; //x = input in [-1..1]
			//float k = 2*amount/(1.000125f-amount);
			//buf[i] = (1+k)*x/(1+k*fabs(x));
			}
			break;
		case 3:
			if (counter++ >= foldback*127) {
				hold = x;
				counter = 0;
			}
			x = hold;
			break;
		}
		buf[i] = x;
	}
}


float LFO::GetValue(float phase, int waveform, int mode, float speed) {
	float lfoVal = t + phase;
	if (waveform == 3) {
		// requires clamping
		if (lfoVal > 1.0f)
			lfoVal = 1.0f;
	} else {
		if (lfoVal > 1.0f)
			lfoVal -= 1.0f;
	}
	switch (waveform) {
	case 0: //SAW1
		break;
	case 1: //SAW2
		lfoVal = 1 - lfoVal;
		break;
	case 2: //SQUARE
		lfoVal = (lfoVal < 0.5f ? 0.0f : 1.0f);
		break;
	case 3: //S&H (becomes WEIRD when phase > 0, but useful when phase = 1 :) )
		lfoVal = sinf(lfoVal*PI/2) * (heldSample - lastHeldSample) + lastHeldSample;
		break;
	case 4: //SINE
		lfoVal = 0.5f*sinf(lfoVal * 2 * PI)+0.5f;
		break;
	}

	if (t >= 1.0f) {
		t -= 1.0f;
		lastHeldSample = heldSample;
		heldSample = GenerateRandomFloat01();
	}

	// Handle LFO polarity
	switch (mode) {
	case LFOPositive:
		break; //do nothing
	case LFONegative:
		lfoVal = -lfoVal;
		break;
	case LFOBipolar:
		lfoVal = 2*lfoVal - 1.0f;
		break;
	default:
		break;
	}

	// Then STEP the slow thingies
	for (int i = 0; i < 2; i++) {
		float delta = speed;
		t += 0.05f * delta*delta*delta*delta;  //TODO: Use something else than p^4 to shape the parameter?
	}
	return lfoVal;
}

void Phaser::Render(Block buf, float frequency, float feedback, float amount) {
	if (frequency <= 0)
		return;
	if (feedback > 1)
		feedback = 1;
	//update filter coeffs
	// TODO - merge them (they are all the same)
	float a1 = AllpassDelay::ComputeDelay(frequency * frequency);
	FORBLOCK(i) {
		//calculate output
		float x = buf[i] + _zm1 * feedback * 0.96;
		for (int j = 0; j < 6; j++)
			x = _alps[j].Update(x, a1);
		_zm1 = x;
		buf[i] += x * amount;
	}
}

void Reverb::Init(const short len[NUMDELAYLINES]) {
	float *buf;
	int lengthSum = 0;
	for (int k = 0; k < NUMDELAYLINES; k++)
		lengthSum += len[k];
	buffer = new float[lengthSum];
	memset(buffer, 0, lengthSum * sizeof(float));
	buf = buffer;
	for (int k = 0; k < NUMDELAYLINES; k++) {
		length[k]  = len[k];
		buffers[k] = buf;
		buf += length[k];
	}
	memset(pos, 0, NUMDELAYLINES*4);
	memset(low, 0, NUMDELAYLINES*4);
}

void Reverb::Shutdown() {
	delete [] buffer;
}

void Reverb::Render(Block in, float halfLife, float pole) {
	float c[NUMDELAYLINES];
	float factor = log(2.0f) / halfLife;
	float f = 1.f; //pole < 0.5f ? 1.0f : float( 1.0f - pole ) / float( pole );
	for (int k = 0; k < NUMDELAYLINES; k++)
		c[k] = (f * exp(-(float)(length[k]) * factor));

	Block temp;
	FORBLOCK(u) {
		temp[u] = 0;
		for (int k = 0; k < NUMDELAYLINES; k++) {
			float &val = buffers[k][pos[k]];
			temp[u] += val;
			low[k] += pole * (val - low[k]);
			val = (low[k] * c[k]) + in[u];
			pos[k]++;
			if (pos[k] >= length[k])
				pos[k]=0;
		}
	}
	memcpy(in, temp, 4*BLOCKSIZE);
}

void Reverb::Silence() {
	for (int i = 0; i < NUMDELAYLINES; i++) {
		memset(buffers[i], 0, length[i] * sizeof(int));
	}
	memset(low,0,sizeof(int)*NUMDELAYLINES);
	memset(pos,0,sizeof(int)*NUMDELAYLINES);
}

/*
Public source code by alex@smartelectronix.com
Simple example of implementation of formant filter
Vowelnum can be 0,1,2,3,4 <=> A,E,I,O,U
Good for spectral rich input like saw or square
*/

// VOWEL COEFFICIENTS
const double coeff[5][11] = {
	{ 8.11044e-06,
	8.943665402,    -36.83889529,    92.01697887,    -154.337906,    181.6233289,
	-151.8651235,   89.09614114,    -35.10298511,    8.388101016,    -0.923313471  ///A
	},
	{4.36215e-06,
	8.90438318,    -36.55179099,    91.05750846,    -152.422234,    179.1170248,  ///E
	-149.6496211,87.78352223,    -34.60687431,    8.282228154,    -0.914150747
	},
	{ 3.33819e-06,
	8.893102966,    -36.49532826,    90.96543286,    -152.4545478,    179.4835618,
	-150.315433,    88.43409371,    -34.98612086,    8.407803364,    -0.932568035  ///I
	},
	{1.13572e-06,
	8.994734087,    -37.2084849,    93.22900521,    -156.6929844,    184.596544,   ///O
	-154.3755513,    90.49663749,    -35.58964535,    8.478996281,    -0.929252233
	},
	{4.09431e-07,
	8.997322763,    -37.20218544,    93.11385476,    -156.2530937,    183.7080141,  ///U
	-153.2631681,    89.59539726,    -35.12454591,    8.338655623,    -0.910251753
	}
};

// Basically a massive FIR. TODO: loop & make generic.
float formant_filter(double *memory, const float in, int vowelnum) {
	float res = (float) (
		coeff[vowelnum][0]  * in +
		coeff[vowelnum][1]  * memory[0] +  
		coeff[vowelnum][2]  * memory[1] +
		coeff[vowelnum][3]  * memory[2] +
		coeff[vowelnum][4]  * memory[3] +
		coeff[vowelnum][5]  * memory[4] +
		coeff[vowelnum][6]  * memory[5] +
		coeff[vowelnum][7]  * memory[6] +
		coeff[vowelnum][8]  * memory[7] +
		coeff[vowelnum][9]  * memory[8] +
		coeff[vowelnum][10] * memory[9]);
	memory[9] = memory[8];
	memory[8] = memory[7];
	memory[7] = memory[6];
	memory[6] = memory[5];
	memory[5] = memory[4];
	memory[4] = memory[3];
	memory[3] = memory[2];
	memory[2] = memory[1];                    
	memory[1] = memory[0];
	memory[0] = (double) res;
	return res;
}

void FormantFilter::Render(Block where, int vowel) {
	FORBLOCK(i) {
		where[i] = formant_filter(memory, where[i], vowel);
	}
}

}  // namespace